#include "main.h"
#include "cmsis_os.h"
#include "motor_protocol.h"
#include "usr_protocol.h"

void update_motor_error_status(uint8_t uart_channel, uint8_t id, int16_t motor_error)
{

    uint8_t array_index = id - 1;
    // Clear error bit first
    motor_status_reg_base_addr[uart_channel][array_index] &= ~MOTOR_ERROR_MASK;
    
    // Set error bit if motor_error is non-zero
    if (motor_error > 0) {
        motor_status_reg_base_addr[uart_channel][array_index] |= MOTOR_ERROR_MASK;
    }
}

void update_motor_status(uint8_t uart_channel, uint8_t id, uint16_t motor_status)
{
    uint8_t array_index = id - 1;
    
    motor_status_reg_base_addr[uart_channel][array_index] &= ~MOTOR_STATUS_MASK;
    if(motor_status > 0)
    {
        motor_status_reg_base_addr[uart_channel][array_index] |= MOTOR_STATUS_MASK;
    }
        
}
void update_motor_testing_up_down_limit(uint8_t uart_channel, uint8_t id, uint16_t motor_status)
{
    uint8_t array_index = id - 1;
    
    motor_status_reg_base_addr[uart_channel][array_index] &= ~MOTOR_TEST_UP_DOWN_LIMIT_MASK;
    if(motor_status > 0)
    {
        motor_status_reg_base_addr[uart_channel][array_index] |= MOTOR_TEST_UP_DOWN_LIMIT_MASK;
    }
}

void update_motor_testing_up_down_limit_done(uint8_t uart_channel, uint8_t id, uint16_t motor_status)
{
    uint8_t array_index = id - 1;
    
    motor_status_reg_base_addr[uart_channel][array_index] &= ~MOTOR_TEST_UP_DOWN_LIMIT_DONE_MASK;
    if(motor_status > 0)
    {
        motor_status_reg_base_addr[uart_channel][array_index] |= MOTOR_TEST_UP_DOWN_LIMIT_DONE_MASK;
    }
}


void update_motor_up_limit_reached(uint8_t uart_channel, uint8_t id, uint16_t motor_status)
{

    uint8_t array_index = id - 1;
    
    motor_status_reg_base_addr[uart_channel][array_index] &= ~MOTOR_TEST_UP_LIMIT_DONE_MASK;
    if(motor_status > 0)
    {
        motor_status_reg_base_addr[uart_channel][array_index] |= MOTOR_TEST_UP_LIMIT_DONE_MASK;
    }

}

void update_motor_down_limit_reached(uint8_t uart_channel, uint8_t id, uint16_t motor_status)
{
    
    uint8_t array_index = id - 1;
    
    motor_status_reg_base_addr[uart_channel][array_index] &= ~MOTOR_TEST_DOWN_LIMIT_DONE_MASK;
    if(motor_status > 0)
    {
        motor_status_reg_base_addr[uart_channel][array_index] |= MOTOR_TEST_DOWN_LIMIT_DONE_MASK;
    }

}






// 报文最小长度 (头2字节 + 命令1字节 + 校验1字节)
#define MIN_PACKET_SIZE 4

// CRC校验计算函数
uint8_t calculate_checksum(const uint8_t *data, uint16_t length) 
{
    uint8_t sum = 0;
    for (uint16_t i = 0; i < length; i++) {
        sum += data[i];
    }
    return sum;
}

// 发送命令帧函数
uint16_t build_command_frame(uint8_t* buffer, uint8_t id,uint8_t Cmd, uint16_t Index, const uint8_t* pdata, uint8_t DataLen)
{
    uint16_t pos = 0;
    uint8_t i;
    
    buffer[pos++] = 0x55;                 // 帧头
    buffer[pos++] = 0xaa;                 // 帧头
    buffer[pos++] = DataLen + 3;          // 数据长度
    buffer[pos++] = id;     // ID
    buffer[pos++] = Cmd;                  // 命令类型
    buffer[pos++] = Index & 0xff;         // 索引低字节
    buffer[pos++] = (Index >> 8) & 0xff;  // 索引高字节
    
    // 复制数据
    for (i = 0; i < DataLen; i++)
    {
        buffer[pos++] = pdata[i];
    }
    
    // 计算校验和
    buffer[pos] = calculate_checksum(&buffer[2], pos - 2);  // 校验和
    pos++;
    
    return pos;  // 返回帧总长度
}



// 协议解析函数
uint16_t process_protocol_data(uint8_t uart_channel, const uint8_t* data, uint16_t length) 
{
    int16_t actual_current = 0;
    int16_t actual_position = 0;
    int16_t actual_temperture = 0;
    int16_t max_current = 0;
    int16_t max_temperture = 0;
    int16_t motor_error = 0;
    int16_t set_position = 0;    
    int16_t postion_erro =0;
    int16_t up_down_limit_status = 0;
    int16_t up_limit_position =0;
    int16_t down_limit_position =0;

    // 检查数据长度是否足够
    if (length < MIN_PACKET_SIZE) {
        return 0; // 数据不完整
    }
    
    // 检查头部
    if (data[0] != PROTOCOL_HEADER1 || data[1] != PROTOCOL_HEADER2) {
        return 0; // 头部错误
    }
    
    uint8_t data_length = data[2]; // 数据长度
    // 提取命令
    uint8_t id = data[3];
    // 提取ID
    uint8_t cmd = data[4];

    uint16_t reg_addr = data[5] | (data[6] << 8);
    // 校验和验证
    uint8_t received_checksum = data[length - 1];
    uint8_t calculated_checksum = calculate_checksum(&data[2], data_length +2);
    
    if (received_checksum != calculated_checksum) {
        return 0; // 校验错误
    }
    if (id < MAX_ID) {

        device_online_status_base_addr[uart_channel][id-1] = (int16_t)SLAVE_ONLINE; // 读取寄存器值
        
    }



    switch (cmd)
    {
    case CMD_READ_STATUS:    
        break;
    case CMD_READ_REG:
        if(reg_addr == MOTOR_CMD_LIMIT_UPPER)
            motor_uplimit_positon_base_addr[uart_channel][id-1] = (data[8] << 8) | data[7]; // 读取寄存器值
        if(reg_addr == MOTOR_CMD_LIMIT_LOWER)
            motor_downlimit_position_base_addr[uart_channel][id-1] = (data[8] << 8) | data[7]; // 读取寄存器值  
        if(reg_addr == MOTOR_CMD_STATE_WORD)
        {
            up_down_limit_status = (data[8] << 8) | data[7]; // 读取寄存器值 
            if(up_down_limit_status & (1<<6))
            {
                //update_motor_up_limit_done(uart_channel,id,MOTOR_TEST_UP_LIMIT_DONE_MASK);
            }
            if(up_down_limit_status & (1<<7))
            {
                //update_motor_down_limit_done(uart_channel,id,MOTOR_TEST_DOWN_LIMIT_DONE_MASK);
            }
        }
        return 1;
    default:
        break;
    }

    actual_temperture = (data[18] << 8) | data[17]; // 读取寄存器值
    actual_current = (data[10] << 8) | data[9]; // 读取寄存器值
    motor_error = (data[16] << 8) | data[15]; // 读取寄存器值
    actual_position = (data[8] << 8) | data[7]; // 读取寄存器值
    motor_actual_positon_base_addr[uart_channel][id-1] = actual_position;
    motor_actual_current_base_addr[uart_channel][id-1] = actual_current;
    motor_actual_erro_base_addr[uart_channel][id-1] = motor_error;
    motor_actual_temp_base_addr[uart_channel][id-1] =actual_temperture;

    update_motor_error_status(uart_channel,id,motor_error);

    set_position = motor_setpoint_positon_base_addr[uart_channel][id-1];
    postion_erro = set_position - actual_position;
    if( abs(postion_erro) < 500 )
    {
        update_motor_status(uart_channel,id,0);
    }
    up_limit_position = motor_uplimit_positon_base_addr[uart_channel][id-1];
    postion_erro = up_limit_position - actual_position;
    if( abs(postion_erro) < 500 )
    {
        update_motor_up_limit_reached(uart_channel,id,1);
    }
    else
    {
        update_motor_up_limit_reached(uart_channel,id,0);

    }


    down_limit_position = motor_downlimit_position_base_addr[uart_channel][id-1];
    postion_erro = down_limit_position - actual_position;
    if( abs(postion_erro) < 500 )
    {
        update_motor_down_limit_reached(uart_channel,id,1);
    }
    else
    {
        update_motor_down_limit_reached(uart_channel,id,0);

    }


    max_current = motor_max_record_current_base_addr[uart_channel][id-1];
    if(actual_current > max_current)
    {
        motor_max_record_current_base_addr[uart_channel][id-1] = actual_current;
    }
    max_temperture = motor_max_record_temp_base_addr[uart_channel][id-1];
    if(actual_temperture > max_temperture)
    {
        motor_max_record_temp_base_addr[uart_channel][id-1] = actual_temperture;
    }

    return 1;
}

